#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy
import numpy as np
# import keyboard
import termios
import tty
from moveit_commander import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from mqtt_integration.msg import TrajectoryInfo

def getKey():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        key = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return key

class TrajectoryPublisher:
    def __init__(self):
        self.pub = rospy.Publisher('trajectory_info', TrajectoryInfo, queue_size=10)
        self.current_trajectory = 0
        self.total_executions = 0
        rospy.Timer(rospy.Duration(2), self.publish_info_timer)  # 每秒调用一次publish_info_timer

    def publish_info(self):
        info = TrajectoryInfo()
        info.current_trajectory = self.current_trajectory
        info.total_executions = self.total_executions
        # rospy.loginfo(f"Publishing: {info}")
        self.pub.publish(info)

    def publish_info_timer(self, event):
        self.publish_info()


class MoveItCartesianDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)

         # 初始化TrajectoryPublisher
        self.trajectory_publisher = TrajectoryPublisher()

        # 是否需要使用笛卡尔空间的运动规划
        self.cartesian = rospy.get_param('~cartesian', True)

        self.arm = MoveGroupCommander('manipulator')

        # 当运动规划失败后，允许重新规划
        self.arm.allow_replanning(True)
       
        # 设置目标位置所使用的参考坐标系
        self.arm.set_pose_reference_frame('base_link')
               
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        self.arm.set_goal_position_tolerance(0.0001)
        self.arm.set_goal_orientation_tolerance(0.0001)
       
        # 设置允许的最大速度和加速度
        self.arm.set_max_acceleration_scaling_factor(0.1)
        self.arm.set_max_velocity_scaling_factor(0.1)
       
        # 获取终端link的名称
        end_effector_link = self.arm.get_end_effector_link()

        # 控制机械臂先回到初始化位置
        self.arm.set_named_target('home')
        self.arm.go()
        rospy.sleep(1)
                                               
        # 获取当前位姿数据为机械臂运动的起始位姿
        self.start_pose = self.arm.get_current_pose(end_effector_link).pose

    def execute_trajectory(self, trajectory_method, trajectory_number):
        self.trajectory_publisher.total_executions += 1
        self.trajectory_publisher.current_trajectory = trajectory_number
        # self.trajectory_publisher.publish_info()
        trajectory_method()
        self.trajectory_publisher.current_trajectory = 0
        # self.trajectory_publisher.publish_info()

    def trajectory_1(self):
        print("Executing Trajectory 1")
               
        waypoints = []

        if self.cartesian:
            waypoints.append(self.start_pose)

        wpose = deepcopy(self.start_pose)

        # 1.1 8/0/0/0/0
        wpose.position.x = 0.031240351682225935
        wpose.position.y = 0.11657024706040157
        wpose.position.z = 0.35980622423789693
        wpose.orientation.x = -5.071551442841066e-05
        wpose.orientation.y = 3.5433726337292932e-06
        wpose.orientation.z = -0.06971326667696516
        wpose.orientation.w = 0.9975670693565458
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.2 13/0/0/0/0
        wpose.position.x = 0.0505023471504527
        wpose.position.y = 0.11297343497763537
        wpose.position.z = 0.3598345306461554
        wpose.orientation.x = 1.2196291363768012e-05
        wpose.orientation.y = -1.3892017988867915e-06
        wpose.orientation.z = -0.1132341862000632
        wpose.orientation.w = 0.9935683262488444
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.3 20/0/0/0/0
        wpose.position.x = 0.07677171222786823
        wpose.position.y = 0.10520350821222695
        wpose.position.z = 0.3597997151177298
        wpose.orientation.x = -6.436007182513625e-05
        wpose.orientation.y = 1.1346520069511344e-05
        wpose.orientation.z = -0.17371348644207885
        wpose.orientation.w = 0.9847962329117508
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.4 20/0/0/0/0
        wpose.position.x = 0.09836519341012566
        wpose.position.y = 0.09604870008925734
        wpose.position.z = 0.35980923806960247
        wpose.orientation.x = -4.298644666378746e-05
        wpose.orientation.y = 9.927224492442027e-06
        wpose.orientation.z = -0.22496258610384628
        wpose.orientation.w = 0.9743674013979969
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.5 26/40/-75/-20/0
        wpose.position.x = 0.05450377887113492
        wpose.position.y = 0.0061391458111090536
        wpose.position.z = 0.12431796491734401
        wpose.orientation.x = -0.4499305923425203
        wpose.orientation.y = 0.1038474123636137
        wpose.orientation.z = -0.1995171072180362
        wpose.orientation.w = 0.8642748989453743
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.6 26/20/-75/-4/0
        wpose.position.x = 0.0775675646742458
        wpose.position.y = 0.05345368227938003
        wpose.position.z = 0.10690652370061177
        wpose.orientation.x = -0.47974382672610255
        wpose.orientation.y = 0.11073544784991857
        wpose.orientation.z = -0.19576329531174247
        wpose.orientation.w = 0.8480921256068593

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)
        
        # 1.7 26/10/-75/8/0
        wpose.position.x = 0.0938519077860496
        wpose.position.y = 0.08688879729082807
        wpose.position.z = 0.09849178456661609
        wpose.orientation.x = -0.4649051989989751
        wpose.orientation.y = 0.10731997236673906
        wpose.orientation.z = -0.19762362378299111
        wpose.orientation.w = 0.8563238188896795

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.8 26/25/-100/32/0
        wpose.position.x = 0.10190871411076276
        wpose.position.y = 0.10342301910338081
        wpose.position.z = 0.10543615980302509
        wpose.orientation.x = -0.3570956933331718
        wpose.orientation.y = 0.08244721964637118
        wpose.orientation.z = -0.20921162000152874
        wpose.orientation.w = 0.9065956208982152

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.9 26/40/-124/55/0
        wpose.position.x = 0.10555546016283122
        wpose.position.y = 0.11076689249765213
        wpose.position.z = 0.11497877279670789
        wpose.orientation.x = -0.24396667093875943
        wpose.orientation.y = 0.056309724689993235
        wpose.orientation.z = -0.217829128250576
        wpose.orientation.w = 0.9433238835426517

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

         # 1.10 26/50/-148/91/0
        wpose.position.x = 0.10967026477008365
        wpose.position.y = 0.11923649152795966
        wpose.position.z = 0.15288573197671254
        wpose.orientation.x = -0.059521952960639925
        wpose.orientation.y = 0.013735821129149126
        wpose.orientation.z = -0.22454881690459033
        wpose.orientation.w = 0.9725462935821657

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.11平移 22/50/-148/91/0
        wpose.position.x = 0.093720763189338
        wpose.position.y = 0.12633408249197853
        wpose.position.z = 0.1528481757837365
        wpose.orientation.x = -0.0600125654349569
        wpose.orientation.y = 0.011661802473411979
        wpose.orientation.z = -0.19045838358154457
        wpose.orientation.w = 0.9797898236236644

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.12平移 18/50/-148/91/0
        wpose.position.x = 0.07735415631251287
        wpose.position.y = 0.1322998604888187
        wpose.position.z = 0.15285187095241054
        wpose.orientation.x = -0.06037918897139795
        wpose.orientation.y = 0.009562981786494297
        wpose.orientation.z = -0.15621742509708259
        wpose.orientation.w = 0.9858291023369846

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.13平移 18/50/-145/91/0
        # 1.13平移 18/50/-140/91/0
        wpose.position.x = 0.07802383093662281
        wpose.position.y = 0.1343371952672179
        wpose.position.z = 0.20087937113738238
        wpose.orientation.x = 0.008600854238775522
        wpose.orientation.y = -0.0013629277011399004
        wpose.orientation.z = -0.15650991738688438
        wpose.orientation.w = 0.9876379971902638

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.14平移 12/50/-140/91/0
        wpose.position.x = 0.05244272759307842
        wpose.position.y = 0.1411959175384837
        wpose.position.z = 0.20088287161086368
        wpose.orientation.x = 0.008684672281378305
        wpose.orientation.y = -0.0009127371922042527
        wpose.orientation.z = -0.10442695409749694
        wpose.orientation.w = 0.994494220514178

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.15往后缩 12/60/-150/91/0
        wpose.position.x = 0.04997896055989197
        wpose.position.y = 0.12957106005078503
        wpose.position.z = 0.17982607489067434
        wpose.orientation.x = 6.074824626952852e-06
        wpose.orientation.y = -6.386726918991548e-07
        wpose.orientation.z = -0.10444034571418614
        wpose.orientation.w = 0.9945311529307617

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 1.16送食 0/-45/0/45/0
        wpose.position.x = 7.049465855556011e-05
        wpose.position.y = 0.28844833329508146
        wpose.position.z = 0.28955458392890854
        wpose.orientation.x = 2.880176438026441e-05
        wpose.orientation.y = -2.323336038735687e-09
        wpose.orientation.z = -7.5493518036726685e-06
        wpose.orientation.w = 0.9999999995567329

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        if self.cartesian:
            fraction = 0.0  
            maxtries = 100   
            attempts = 0    
       
        self.arm.set_start_state_to_current_state()
     
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path (
                                    waypoints,   
                                    0.01,       
                                    0.0,        
                                    True)       
           
            attempts += 1
           
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) + " attempts...")

        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the self.arm.")
            new_traj = self.scale_trajectory_speed(plan, 1)
            self.arm.execute(new_traj)
            rospy.loginfo("Path execution complete.")

        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        rospy.sleep(1)
        pass

    def trajectory_2(self):
        print("Executing Trajectory 2")
        waypoints = []

        if self.cartesian:
            waypoints.append(self.start_pose)

        wpose = deepcopy(self.start_pose)

        # 2.1 0/45/-78/-15/0
        wpose.position.x = 3.5861710759860426e-05
        wpose.position.y = 0.028616485231642078
        wpose.position.z = 0.13710251958735695
        wpose.orientation.x = -0.4067103800735914
        wpose.orientation.y = 2.2985965300968713e-05
        wpose.orientation.z = 2.969355709805773e-05
        wpose.orientation.w = 0.9135571494604664
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.2 0/20/-95/30/0
        wpose.position.x = 8.728001172618719e-05
        wpose.position.y = 0.13131633025073322
        wpose.position.z = 0.10369840778116768
        wpose.orientation.x = -0.38267036635991264
        wpose.orientation.y = -7.103024282056723e-06
        wpose.orientation.z = -8.142245469442147e-05
        wpose.orientation.w = 0.9238849409043862

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.3 0/25/-100/35/0
        wpose.position.x = 9.140326807020497e-05
        wpose.position.y = 0.13455500316757113
        wpose.position.z = 0.11437586745558012
        wpose.orientation.x = -0.342037949554521
        wpose.orientation.y = 2.535275147950968e-05
        wpose.orientation.z = -8.22146729376945e-05
        wpose.orientation.w = 0.9396861357190094

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.4 0/35/-115/45/0
        wpose.position.x = 5.6229507373585196e-05
        wpose.position.y = 0.1302788020557089
        wpose.position.z = 0.10944191842032809
        wpose.orientation.x = -0.30077110079880054
        wpose.orientation.y = 2.203118207948195e-05
        wpose.orientation.z = -7.178536098892225e-06
        wpose.orientation.w = 0.9536963585897629

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.5 0/40/-128/60/0
        wpose.position.x = 5.487498284877057e-05
        wpose.position.y = 0.13754329523185033
        wpose.position.z = 0.11010238522046613
        wpose.orientation.x = -0.24193478789985198
        wpose.orientation.y = -1.4565110866545777e-05
        wpose.orientation.z = -1.7205075956062853e-05
        wpose.orientation.w = 0.9702925115117074 

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.6 0/45/-138/75/0
        wpose.position.x = 4.321563529910791e-05
        wpose.position.y = 0.14423559342959116
        wpose.position.z = 0.12889714851745873
        wpose.orientation.x = -0.15637593948448725
        wpose.orientation.y = 5.154383514451827e-07
        wpose.orientation.z = 1.546783750207626e-05
        wpose.orientation.w = 0.987697608233828

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.7 0/50/-148/91/0
        wpose.position.x = 4.2853274150827024e-05
        wpose.position.y = 0.1445731359325109
        wpose.position.z = 0.15291118015053048
        wpose.orientation.x = -0.061034731002165755
        wpose.orientation.y = -2.6479789788413422e-06
        wpose.orientation.z = 1.066907802242997e-05
        wpose.orientation.w = 0.9981356428315004

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.8平移 8/50/-148/91/0
        wpose.position.x = 0.034841145092543116
        wpose.position.y = 0.14213259134602313
        wpose.position.z = 0.15293437713280872
        wpose.orientation.x = -0.060845223216524726
        wpose.orientation.y = 0.004253728610493487
        wpose.orientation.z = -0.06960235546274252
        wpose.orientation.w = 0.9957084295709652

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.9抬高 8/50/-140/91/0
        wpose.position.x = 0.03516114345322087
        wpose.position.y = 0.1442399030862182
        wpose.position.z = 0.2009049393174548
        wpose.orientation.x = 0.008752860398081977
        wpose.orientation.y = -0.0006119923917171341
        wpose.orientation.z = -0.06978126514181372
        wpose.orientation.w = 0.9975237280061924

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.10再平移 12/50/-140/91/0
        wpose.position.x = 0.05248577625639087
        wpose.position.y = 0.1411793575598751
        wpose.position.z = 0.20091323583412754
        wpose.orientation.x = 0.00872645906364369
        wpose.orientation.y = -0.0009173374470786504
        wpose.orientation.z = -0.10452520340421084
        wpose.orientation.w = 0.9944835289020765

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        #  2.11后缩 12/50/-140/91/0
        wpose.position.x = 0.050016239763882725
        wpose.position.y = 0.12944864700583408
        wpose.position.z = 0.18368757889362824
        wpose.orientation.x = 0.008583267144780294
        wpose.orientation.y = -0.00090200032989099
        wpose.orientation.z = -0.10458263395943276
        wpose.orientation.w = 0.9944787512031786

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 2.8送食
        wpose.position.x = 7.049465855556011e-05
        wpose.position.y = 0.28844833329508146
        wpose.position.z = 0.28955458392890854
        wpose.orientation.x = 2.880176438026441e-05
        wpose.orientation.y = -2.323336038735687e-09
        wpose.orientation.z = -7.5493518036726685e-06
        wpose.orientation.w = 0.9999999995567329

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        if self.cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            self.arm.set_start_state_to_current_state()
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = self.arm.compute_cartesian_path(
                    waypoints, 0.01, 0.0, True)
                attempts += 1
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts.")

            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
                new_traj = self.scale_trajectory_speed(plan, 1)
                self.arm.execute(new_traj)
                rospy.loginfo("Path execution complete.")
            else:
                rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")

            rospy.sleep(1)

        pass

    def trajectory_3(self):
        print("Executing Trajectory 3")
        # 初始化路点列表
        waypoints = []

        # 将初始位姿加入路点列表
        if self.cartesian:
            waypoints.append(self.start_pose)

        # 设置路点数据，并加入路点列表
        wpose = deepcopy(self.start_pose)
        # 3.1 -8/0/0/0/0
        wpose.position.x = -0.031168722768712603
        wpose.position.y = 0.11657380103657414
        wpose.position.z = 0.3598317930311014
        wpose.orientation.x = 6.1553480981672e-06
        wpose.orientation.y = 4.3050536038942754e-07
        wpose.orientation.z = 0.06977623706470132
        wpose.orientation.w = 0.9975626680580107
        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.2 -13/0/0/0/0
        wpose.position.x = -0.050409755512666105
        wpose.position.y = 0.11298088749099783
        wpose.position.z = 0.3598469067566341
        wpose.orientation.x = 3.9616285959787196e-05
        wpose.orientation.y = 4.51585058841313e-06
        wpose.orientation.z = 0.11324750712240661
        wpose.orientation.w = 0.9935668072861118

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)
        
        # 3.3 -20/0/0/0/0
        wpose.position.x = -0.07664706569283344
        wpose.position.y = 0.10523798361178978
        wpose.position.z = 0.3598212338774806
        wpose.orientation.x = -1.7106647426589924e-05
        wpose.orientation.y = -3.0177083734344027e-06
        wpose.orientation.z = 0.17363835161486693
        wpose.orientation.w = 0.9848094854065572

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.4 -26/0/-0/0/0
        wpose.position.x = -0.09823540746879462
        wpose.position.y = 0.0960638282344723
        wpose.position.z = 0.35983608304534126
        wpose.orientation.x = 1.533401091891952e-05
        wpose.orientation.y = 3.540172588279489e-06
        wpose.orientation.z = 0.22491285700790822
        wpose.orientation.w = 0.9743788824193983

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.5 -26/40/-75/-20/0
        wpose.position.x = -0.05442712271251495
        wpose.position.y = 0.006189099692509581
        wpose.position.z = 0.12431178334557616
        wpose.orientation.x = -0.44996849402342126
        wpose.orientation.y = -0.10379707122178664
        wpose.orientation.z = 0.19951514382580832
        wpose.orientation.w = 0.8642616674226856

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

         # 3.6 -26/20/-75/-4/0
        wpose.position.x = -0.07746814540877038
        wpose.position.y = 0.053478234306858746
        wpose.position.z = 0.10686112144599383
        wpose.orientation.x = -0.47984713180284405
        wpose.orientation.y = -0.11075659848244414
        wpose.orientation.z = 0.19574938248383222
        wpose.orientation.w = 0.8480341297674175

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)


        # 3.7 -26/10/-75/8/0
        wpose.position.x = -0.09378779016725308
        wpose.position.y = 0.08690792190055481
        wpose.position.z = 0.09847789076652683
        wpose.orientation.x = -0.4649318272946334
        wpose.orientation.y = -0.10731542198475902
        wpose.orientation.z = 0.1976925996181981
        wpose.orientation.w = 0.8562940103894837

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.8 -26/25/-100/32/0
        wpose.position.x = -0.10185816545490319
        wpose.position.y = 0.10347783908022863
        wpose.position.z = 0.10546227953481535
        wpose.orientation.x = -0.3570241013462167
        wpose.orientation.y = -0.0824241581443727
        wpose.orientation.z = 0.2092844039348642
        wpose.orientation.w = 0.9066091150444863

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.9  轨迹点2.2 -26/40/-124/55/0
        wpose.position.x = -0.1054117011712569
        wpose.position.y = 0.1108467509634318
        wpose.position.z = 0.11499687384853347
        wpose.orientation.x = -0.2439275718634523
        wpose.orientation.y = -0.056312765155181785
        wpose.orientation.z = 0.21768761051587665
        wpose.orientation.w = 0.9433664804270214

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

         # 3.10 轨迹点2.3 -26/50/-148/91/0
        wpose.position.x = -0.10954038258269447
        wpose.position.y = 0.1192944483298219
        wpose.position.z = 0.1528943970034999
        wpose.orientation.x = -0.05946987362727919
        wpose.orientation.y = -0.013733472824132257
        wpose.orientation.z = 0.22443885251374499
        wpose.orientation.w = 0.9725748954899349

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.11平移 -22/50/-148/91/0
        wpose.position.x = -0.0936176385542106
        wpose.position.y = 0.12637390058752562
        wpose.position.z = 0.1529021353704747
        wpose.orientation.x = -0.05990584566501952
        wpose.orientation.y = -0.011646877158218976
        wpose.orientation.z = 0.19042512117381682
        wpose.orientation.w = 0.9798029971037838

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.12平移 -18/50/-148/91/0
        wpose.position.x = -0.07720428524418477
        wpose.position.y = 0.13233934404644604
        wpose.position.z = 0.1529294124429962
        wpose.orientation.x = -0.0602170034778715
        wpose.orientation.y = -0.009538775824991285
        wpose.orientation.z = 0.1560948211728744
        wpose.orientation.w = 0.9858586770176114

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.13平移 18/50/-145/91/0
        # 3.13平移 -18/50/-140/91/0
        wpose.position.x = -0.07786707453417932
        wpose.position.y = 0.13437065705025183
        wpose.position.z = 0.20096614055894246
        wpose.orientation.x = 0.008781171200801458
        wpose.orientation.y = 0.0013903418009307096
        wpose.orientation.z = 0.15639470372149342
        wpose.orientation.w = 0.9876546231501603

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.14平移 -12/50/-140/91/0
        wpose.position.x = -0.05239355358617451
        wpose.position.y = 0.14118167702093876
        wpose.position.z = 0.2009817571139045
        wpose.orientation.x = 0.00889233848707391
        wpose.orientation.y = 0.0009341414512527173
        wpose.orientation.z = 0.10455471516283729
        wpose.orientation.w = 0.9944789415735249

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.15往后缩 -12/60/-150/91/0
        wpose.position.x = -0.04989556381254159
        wpose.position.y = 0.1294807134301522
        wpose.position.z = 0.1837350610560631
        wpose.orientation.x = 0.008634658197445457
        wpose.orientation.y = 0.0009074450860638917
        wpose.orientation.z = 0.10452561069753129
        wpose.orientation.w = 0.9944842964720646

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        # 3.16送食 0/-45/0/45/0
        wpose.position.x = 7.049465855556011e-05
        wpose.position.y = 0.28844833329508146
        wpose.position.z = 0.28955458392890854
        wpose.orientation.x = 2.880176438026441e-05
        wpose.orientation.y = -2.323336038735687e-09
        wpose.orientation.z = -7.5493518036726685e-06
        wpose.orientation.w = 0.9999999995567329

        if self.cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            self.arm.set_pose_target(wpose)
            self.arm.go()
            rospy.sleep(1)

        if self.cartesian:
            fraction = 0.0   
            maxtries = 100   
            attempts = 0     
       
        self.arm.set_start_state_to_current_state()
     
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path (
                                    waypoints, 
                                    0.01,       
                                    0.0,         
                                    True)      
           
            attempts += 1
           
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                     
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the self.arm.")
            new_traj = self.scale_trajectory_speed(plan, 1)
            self.arm.execute(new_traj)
            rospy.loginfo("Path execution complete.")
            
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        rospy.sleep(1)
        pass

    def scale_trajectory_speed(self, traj, scale):
       
       # Create a new trajectory object
       new_traj = RobotTrajectory()
       
       # Initialize the new trajectory to be the same as the input trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
 
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
       
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
        
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
       
       # Cycle through all points and joints and scale the time from start,
       # as well as joint speed and acceleration
       for i in range(n_points):
           
           point = JointTrajectoryPoint()
           
           # The joint positions are not scaled so pull them out first
           point.positions = traj.joint_trajectory.points[i].positions

           # Next, scale the time_from_start for this point
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
           # Get the joint velocities for this point
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
           # Get the joint accelerations for this point
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
           # Scale the velocity and acceleration for each joint at this point
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
        
           # Store the scaled trajectory point
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj

    def go_home(self):
        print("Returning to home position")
        self.arm.set_named_target('home')
        self.arm.go()
        rospy.sleep(1)

    def shutdown(self):
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    print("Press '1', '2', or '3' to execute  trajectories, or '0' to return to home position")
    demo = MoveItCartesianDemo()  # 创建实例
    while True:
        try:
            key = getKey()
            if key == '1':
                demo.execute_trajectory(demo.trajectory_1,1)
            elif key == '2':
                demo.execute_trajectory(demo.trajectory_2,2)
            elif key == '3':
                demo.execute_trajectory(demo.trajectory_3,3)
            elif key == '0':
                demo.go_home()
            elif key == 'q':
                print("Exiting...")
                break
        except Exception as e:
            print("Error:", e)
            break

    demo.shutdown()  # 正确关闭moveit